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Abstract 


We  introduce  Simultaneous  Localization  and  Tracking  (SLAT),  the  problem  of  tracking  a 
target  in  a  sensor  network  while  simultaneously  localizing  and  calibrating  the  nodes  of  the 
network.  Our  proposed  solution,  LaSLAT,  is  a  Bayesian  filter  providing  on-line  probabilistic 
estimates  of  sensor  locations  and  target  tracks.  It  does  not  require  globally  accessible  beacon 
signals  or  accurate  ranging  between  the  nodes.  When  applied  to  a  network  of  27  sensor  nodes, 
our  algorithm  can  localize  the  nodes  to  within  one  or  two  centimeters. 
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1  Introduction 


Many  sensor  network  applications,  such  as  tracking  moving  targets  over  large  regions,  require  that 
the  sensor  nodes  be  calibrated  and  that  their  locations  be  known.  Due  to  the  scale  of  the  deployment 
in  many  applications,  it  is  impractical  to  rely  on  careful  placement  or  uniform  arrangement  of  sensor 
nodes.  Because  GPS  is  unavailable  indoors  and  range  information  between  nodes  based  on  radio 
is  often  unreliable,  automatic  localization  of  nodes  is  challenging.  Furthermore,  spatially  varying 
environmental  factors  preempt  a  full  calibration  at  the  factory  and  require  that  some  of  the  sensor 
parameters  be  calibrated  on  the  field  after  deployment.  For  sensor  networks  deployed  to  track 
moving  targets,  some  authors  have  suggested  localizing  the  network  using  a  moving  target  (or 
mobile )  [1-3].  This  is  an  attractive  solution  since  it  requires  no  additional  hardware  on  the  nodes 
themselves.  However,  these  methods  require  that  the  position  of  the  mobile  be  known  at  all  times. 
In  this  paper,  we  show  how  to  track  the  position  of  an  unconstrained  mobile  while  localizing  and 
calibrating  the  sensor  nodes. 

This  problem,  which  we  label  Simultaneous  Localization  And  Tracking  (SLAT),  is  analogous  to 
SLAM,  where  a  robot  localizes  itself  within  a  map  of  the  environment  while  concurrently  building 
this  map.  Over  the  past  decade,  SLAM  has  witnessed  a  surge  in  its  efficacy  and  performance 
due  in  part  to  the  application  of  Bayesian  methods  [4-9].  We  adapt  some  of  these  methods  to 
the  SLAT  problem  in  the  form  of  a  Bayesian  filter  that  uses  range  measurements  to  update  a 
joint  probability  distribution  over  the  positions  of  the  nodes,  the  trajectory  of  the  mobile,  and  the 
calibration  parameters  of  the  network.  To  avoid  some  of  the  representational  and  computational 
complexity  of  general  Bayesian  filtering,  we  use  Laplace’s  method  to  approximate  our  state  with 
a  Gaussian  after  incorporating  each  batch  of  measurements.  Accordingly,  we  call  our  algorithm 
LaSLAT. 

LaSLAT  inherits  many  desirable  properties  from  the  Bayesian  framework.  The  probabilistic 
model  used  in  LaSLAT  insures  that  measurement  noise  is  averaged  out  as  more  measurements 
become  available,  improving  localization  and  tracking  accuracy  in  high-traffic  areas.  The  filtering 
framework  incorporates  measurements  in  small  batches,  providing  online  estimates  of  all  locations, 
calibration  parameters,  and  their  uncertainties.  This  speeds  up  the  convergence  of  the  algorithm  and 
reduces  impact  on  the  network.  Since  uncertainties  are  known,  mobiles  can  be  dispatched  on-line 
to  improve  localization  estimates  in  regions  where  localization  uncertainty  is  high.  These  mobiles 
may  move  arbitrarily  through  the  environment,  with  no  constraint  on  their  trajectory  or  velocity, 
and  multiple  mobiles  may  be  used  simultaneously  to  expedite  surveying.  Ancillary  localization 
information,  such  as  position  estimates  from  GPS,  beacons,  or  radio-based  ranging,  can  also  be  easily 
incorporated  into  this  framework.  Our  algorithm  is  fast  and  permits  a  distributed  implementation 
because  it  operates  on  sparse  inverse  covariance  matrices  rather  than  dense  covariance  matrices 
themselves.  When  the  user  does  not  specify  a  coordinate  system,  LaSLAT  recovers  locations  in  a 
coordinate  system  that  is  correct  up  to  a  translation,  rotation,  and  possible  reflection. 

We  demonstrate  these  features  by  accurately  localizing  a  dense  network  of  27  nodes  to  within 
one  or  two  centimeters.  The  nodes  are  wireless  Crickets  [10]  capable  of  measuring  their  distance  to  a 
moving  beacon  using  a  combination  of  ultrasound  and  radio  pulses.  In  a  larger  and  sparser  network, 
we  localize  nodes  to  within  about  eight  centimeters.  In  both  cases,  a  measurement  bias  parameter 
is  accurately  calibrated  for  all  nodes.  Finally,  we  present  initial  results  from  an  experiment  in  three 
dimensional  localization  and  tracking. 
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2  Related  Work 


The  most  common  sensor  network  localization  algorithms  rely  on  range  or  connectivity  measure¬ 
ments  between  pairs  of  nodes  [11-16].  When  such  measurements  are  available,  these  methods  can 
supplement  LaSLAT  by  providing  a  prior  or  an  initial  estimate  for  the  location  of  the  sensors 
(Section  3.5). 

Various  authors  have  used  mobiles  to  localize  sensor  networks  [1-3,17],  but  these  methods  assume 
the  location  of  the  mobile  is  known.  One  exception  is  [2],  which  builds  a  constraint  structure  as 
measurements  become  available.  Compared  to  [2],  we  employ  a  very  extensible  statistical  model 
that  allows  more  realistic  measurement  models.  Our  method  is  most  similar  to  [17],  who  used 
an  Extended  Kalman  Filter  (EKF)  to  track  an  underwater  vehicle  while  localizing  sonar  beacons 
capable  of  measuring  their  range  to  the  vehicle.  We  replace  the  EKF’s  approximate  measurement 
model  with  one  based  on  Laplace’s  method.  This  provides  faster  convergence  and  greater  estimation 
accuracy.  We  also  demonstrate  that  the  Bayesian  filtering  framework  can  calibrate  the  sensor  nodes, 
and  that  the  computation  is  capable  of  distributing  over  the  sensor  nodes  in  a  straightforward  way. 

Our  solution  to  SLAT  adopts  various  important  refinements  to  the  original  Extended  Kalman 
Filter  (EKF)  formulation  of  SLAM  [9].  LaSLAT  processes  measurements  in  small  batches  and 
discards  variables  that  are  no  longer  needed,  as  demonstrated  by  McLauchlan  [18].  Following  [6], 
LaSLAT  operates  on  inverse  covariances  of  Gaussians  rather  than  on  covariances  themselves  to 
speed  up  updates  and  facilitate  distributed  computation. 

3  LaSLAT 

To  process  measurements  on-line  LaSLAT  uses  the  Bayesian  filtering  framework.  Under  this  frame¬ 
work,  as  each  batch  of  measurements  becomes  available,  it  is  used  to  update  a  prior  distribution 
over  sensor  locations,  the  mobile  trajectory,  and  various  sensing  parameters.  The  resulting  posterior 
distribution  is  then  propagated  forward  in  time  using  a  dynamics  model  to  make  it  a  suitable  prior 
for  use  with  the  next  batch  of  measurements. 

In  LaSLAT,  the  posterior  distribution  is  approximated  with  a  Gaussian  using  Laplace’s  method 
[19]  after  incorporating  each  batch  of  measurements.  Consequently,  the  amount  of  state  saved 
between  batches  is  constant  with  respect  to  the  number  of  measurements  taken  in  the  past.  The 
Gaussian  approximation  also  simplifies  propagation  with  the  dynamics  model  and  the  incorporation 
of  the  next  batch  of  measurements. 

3.1  Approximate  Bayesian  Filtering  for  SLAT 

As  the  mobile  moves  through  the  network,  it  periodically  emits  events  which  allow  some  of  the 
sensors  to  measure  their  distances  from  the  mobile. 

Let  ej  denote  the  location  of  the  mobile  generating  the  jth  event.  The  tth  batch  ef  is  a  collection 
of  consecutive  events  ef  =  {em . .  .em+n},  with  e*  denoting  the  jth  event  in  the  tth  batch.  Each 
LaSLAT  iteration  incorporates  the  measurements  from  a  single  batch  of  events. 

Let  s i  =  [sf  sf]  represent  the  unknown  parameters  of  sensor  i ,  with  sf  denoting  the  sensor’s 
position  and  sf  its  calibration  parameters.  Then  s  =  {s,;}  is  the  set  of  all  sensor  parameters.  The 
scalar  y|-  denotes  the  range  measurement  between  sensor  i  and  the  jth  event  in  batch  t ,  with 
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yf  =  {yfj}  the  collection  of  all  range  measurements  in  batch  t.  For  each  batch  t.  e*  and  s  are 
the  unknown  values  that  must  be  estimated.  We  aggregate  these  unknowns  into  a  single  variable 
=  [e*  s]  for  notational  simplicity. 

The  Bayesian  filtering  framework  is  a  non-linear,  non-Gaussian  generalization  of  the  Kalman 
Filter.  For  each  batch  t,  it  computes  the  posterior  distribution  over  sensor  parameters,  s,  and  events 
locations,  e4,  taking  into  account  all  range  measurements  taken  so  far: 

p(x*  |y\y2,...,y*). 


In  LaSLAT,  we  wish  to  update  this  distribution  as  range  measurements  become  available,  and 
discard  measurements  as  soon  as  they  have  been  incorporated.  To  do  this,  one  can  rewrite  the 
distribution  in  terms  of  a  measurement  model  and  a  prior  distribution  derived  from  the  results  of 
the  previous  iteration.  Rewriting  ^(x^y^y2, . . . ,  y*)  as  p(x* \yold,  yt),  we  get  by  Bayes  rule: 

p(xVw,yW(; y.xV*) 

ocp(yt|xSyoW)p(xt|yoW) 

ap(yt|xt)p(xf|yoW),  (1) 

where  proportionality  is  with  respect  to  xh  The  hnal  equality  follows  because  when  the  sensor  and 
mobile  locations  are  known,  the  past  measurements  do  not  provide  any  additional  useful  information 
about  the  new  batch  of  measurements.  The  distribution  p(yt\xt)  is  the  measurement  model:  it 
reflects  the  probability  of  a  set  of  observations  given  a  particular  configuration  of  sensors  and  event 
locations  (Section  3.2). 

The  distribution  p(xt\yold)  summarizes  all  information  collected  prior  to  the  current  batch  of 
measurements,  in  the  form  of  a  prediction  of  xf  and  an  uncertainty  measure.  It  can  be  computed 
from  the  previous  estimate,  p(xt~1\yold),  by  applying  a  dynamic  model: 

p{Ay°ld)=  [  p(xt-1|yoJd)p(xt|xt-1)dxt-1.  (2) 

ix*-1 

The  distribution  p(x*|x*_1)  models  the  dynamics  of  the  configuration  from  one  batch  to  another, 
by  discarding  old  event  locations  and  predicting  the  locations  of  new  events  (Section  3.4). 

When  the  measurement  model  p(yt\xt)  is  not  Gaussian,  the  updates  (1)  and  (2)  become  difficult 
to  compute.  We  handle  the  non-Gaussianity  of  the  measurement  model  by  after  each  batch  ap¬ 
proximating  the  posterior  p(x*| yold)  with  a  Gaussian  distribution  q(xt  \yold )  using  Laplace’s  method 
(Section  3.3).  This  Gaussian  becomes  the  basis  for  the  prior  distribution  for  the  next  batch,  q  is 

much  simpler  to  save  between  batches  than  the  full  posterior  -  in  particular,  it  allows  all  the  old 

measurements  to  be  discarded.  Table  1  summarizes  the  steps  of  LaSLAT. 

Other  approximate  Bayesian  filters  such  as  the  Extended  Kalman  Filter  (EKF)  or  particle  filters 
could  also  be  used  in  place  of  our  Laplacian  method.  The  EKF  differs  from  our  algorithm  because 
it  does  not  perform  a  full  optimization  when  incorporating  each  event.  In  many  cases  this  is  a 
helpful  optimization;  however,  as  we  show  in  section  5,  on  the  SLAT  problem  it  sacrifices  accuracy 
and  speed  of  convergence.  Particle  filters  allow  a  closer  approximation  of  the  posterior  distribution, 
especially  when  the  distribution  is  multimodal.  However,  our  algorithm  seems  to  perform  well  in 
practice  while  requiring  significantly  less  computation. 
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1.  Observe  a  new  batch  of  measurements  yt. 

2.  Represent  the  posterior  p(x4  [y4,  yold)  in  terms  of  the  prior  p(xt\yold)  and  the  measurement 
model  p(yt\xt)  using  Equation  (1). 

3.  Using  Newton-Raphson  [20],  compute  curvature  at  the  mode  of  p(xt\yt ,yuld)  and  use  it  to 
construct  the  approximate  posterior  y(xf  |yf,  yoW).  This  posterior  is  the  estimate  for  the  batch 
t  (Section  3.3). 

4.  Compute  the  prediction  p(x‘t+1  |yf,yoM)  using  g(x*|y*,  yoW)  (Section  3.4). 

5.  Using  the  prediction  as  the  new  prior,  return  to  step  1  to  process  batch  t  +  1. 


Table  1:  One  iteration  of  LaSLAT.  Incorporates  batch  t  and  prepares  to  incorporate  batch  t  +  1. 

3.2  The  measurement  model 

Measurements  influence  localization  and  calibration  estimates  via  the  measurement  model.  A  mea¬ 
surement  model  is  a  probability  distribution  p(yt\xt)  over  a  batch  of  range  measurements,  given  a 
particular  choice  of  the  calibration  parameters  and  positions  for  the  sensors  and  the  mobile.  In  this 
paper,  we  assume  that  each  measurement  is  a  corrupted  version  of  the  true  distance  between  the 
event  and  the  sensor  that  made  the  measurement: 

y\j  =  IIs?  -  ejll  +  si  +  (3) 

where  ||  •  ||  indicates  the  vector  2-norm,  giving  the  Euclidean  distance  between  sf  and  ej.  uf-  is  a 
zero-mean  Gaussian  random  variable  with  variance  ex2,  and  s f  is  a  bias  parameter  that  models  an 
unknown  shift  due  to  a  variable  time  delay  in  the  ranging  algorithm. 

As  defined,  p(y\j |sj,  e*)  is  a  univariate  Gaussian  with  mean  ||sj  —  e*- 1|  +  sf  and  variance  a2.  More 
sophisticated  measurement  models  can  be  substituted  if  necessary.  For  example,  using  a  heavy-tailed 
distribution  such  as  the  student-t  in  place  of  the  Gaussian  would  provide  automatic  attenuation  of 
outlying  measurements  (such  as  those  caused  by  echoes).  Other  calibration  parameters  could  also 
be  included  in  the  measurement  model. 

Since  each  measurement  yh  depends  only  on  the  sensor  s,;  that  took  the  measurement,  and  the 
location  e*  of  the  mobile  when  it  generated  the  event,  the  measurement  model  factorizes  according 
to 

p(yt\xt)  =  np(ytijh,etj),  (4) 

i,j 

where  the  product  is  over  the  sensors  and  the  events  that  they  perceived  in  batch  t.  Equation  (4) 
provides  the  measurement  model  for  a  batch  of  measurements.  Note  that  although  this  distribution 
is  Gaussian  in  the  distances  between  y*  and  xt,  it  is  not  Gaussian  in  yf  and  x*  themselves. 

3.3  Incorporating  Measurements 

The  approximate  Gaussian  posterior  q(xt\yold,yt)  can  be  obtained  from  the  prior  distribution 
p(xt\yold)  and  the  measurement  model  p(yt\xt)  using  Laplace’s  method  [19]. 
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To  fit  an  approximate  Gaussian  distribution  q{x)  to  a  distribution  p(x),  Laplace’s  method  first 
finds  the  mode  x*  of  p(x),  then  computes  the  curvature  of  the  negative  log  posterior  at  x*. 

d2 

The  mean  and  covariance  of  q(x)  are  then  set  to  x*  and  A  respectively.  Notice  that  when  p  is 
Gaussian,  the  resulting  approximation  q  is  exactly  p.  For  other  distributions,  the  Gaussian  q  locally 
matches  the  behavior  of  p  about  its  mode. 

The  mode  finding  problem  can  be  expressed  as: 

x4*  =  argmaxp(x*|y<,yoM) 

=  argmin  —  logp(y;|x<)p(xt|yoW) 

X4 

=  arg min(s  -  p)TQ(s  -  p)  +  V(||sf  -  e*  ||  +  sf  -  yjj)2,  (5) 

s,e4  (7Z  J  J 

hi 

where  p  =  E  [x4 |y*,  yold] ,  and  Q  =  Cov"1  [s\yold] . 

We  use  the  Newton- Raphson  iterative  optimization  algorithm  [20]  to  find  the  mode  x4*,  and  the 
curvature  H  (see  the  Appendix).  Following  Laplace’s  method,  the  mean  E  [x* |yoZrf,  y4]  of  q  is  set  to 
x4*  and  its  inverse  covariance  Cov-1  [x* |yoW,  y4]  is  set  to  H.  Representing  q  using  its  inverse  covari¬ 
ance  allows  us  to  avoid  computing  the  matrix  inverse  H-1  after  adding  each  measurement,  which 
significantly  improves  performance  and  facilitates  a  distributed  implementation  of  our  algorithm 
(Section  4). 

3.4  Dynamics  Model 

In  this  paper,  we  assume  the  mobiles  can  move  arbitrarily  and  that  sensors  do  not  move  over 
time.  When  propagating  the  the  posterior  q(x.f \yold,  y*)  forward  in  time,  we  need  only  retain  the 
components  that  are  useful  for  incorporating  the  next  batch  of  measurements.  Thus,  we  may  remove 
the  estimate  of  the  mobile’s  trajectory  from  batch  t,  but  we  must  incorporate  a  guess  for  the  mobile’s 
path  during  batch  t  +  1.  Therefore,  the  prediction  step  of  Equation  (2)  can  be  written: 

p^+W)  =  p(s,e*+1|yo/<V)  =  p(e*+1)g(s|yoW,y*)  (6) 

q(s\y°ldy)=  f  g(xVW  yVe*. 

Jet 

The  Gaussian  q(xt\yold,yt)  captures  the  posterior  distribution  over  sensor  locations  given  all  mea¬ 
surements  before  yf ,  and  has  already  been  computed  by  the  method  of  section  3.3.  We  obtain 
q(s \yold,yt),  by  marginalizing  out  the  mobile’s  trajectory  during  batch  t.  The  prior  p(et+1)  is 
Gaussian  with  very  broad  covariance,  indicating  that  the  future  trajectory  of  the  mobile  is  un¬ 
known.  In  some  applications,  it  may  be  possible  to  use  past  trajectories  to  make  better  guesses  for 
et+1.  For  maximum  generality,  we  will  not  attempt  to  do  so  in  this  paper,  meaning  that  the  mobile 
is  allowed  to  move  arbitrarily  between  events. 
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The  operations  of  Equation  (6)  can  be  carried  out  numerically  by  operating  on  the  mean  and 
inverse  covariance  of  g(x*|yoW).  First,  partition  according  to  s  and  e*: 

FU{nid]  =  r^[s|yoW]' 
h[*'y  \  [E[et\yold] 

Cov  Lx|y  J  ~  [nets  net  ' 

Marginalizing  out  e*  produces  a  distribution  q(s\yold )  whose  mean  is  the  s  component  of  the  mean 
of  <7(x4  |yoM)  and  whose  inverse  covariance  is 


Cov  1  s|yoZcZ  =Qa-  nsetQjttets 


The  parameters  of  p(xt+1  \yold)  are  those  of  q(s\yold),  augmented  by  zeros  to  account  for  an 
uninformative  prior  on  et+1: 


EU+i  \vold]  =  \E  [s|y°W]" 

L  J  [  o 
Cov-1  [x‘+V“]  =  ’Cov-1  Hj^I  O' 


The  components  of  the  inverse  covariance  of  p(x.t+1\yold)  corresponding  to  et+1  are  set  to  0,  cor¬ 
responding  to  infinite  variance,  which  in  turn  captures  our  lack  of  a  priori  knowledge  about  the 
location  of  the  mobiles  in  the  new  batch.  The  mean  is  arbitrarily  set  to  0.  If  some  information 
is  known  a  priori  about  e*+1,  then  the  last  0  components  of  E  [xt+1|yoW]  and  the  bottom  right  0 


components  of  Cov  1  xt+1|yoW  can  be  used  to  capture  that  knowledge. 


3.5  Prior  Information  and  Initialization 

Prior  information  about  the  sensor  parameters  is  easy  to  incorporate  into  LaSLAT.  Such  informa¬ 
tion  might  be  available  because  the  sensors  were  placed  in  roughly  known  positions,  or  because 
another  less  accurate  source  of  localization  is  available.  Calibration  in  the  factory  might  also  supply 
additional  prior  information. 

If  such  prior  information  is  available  it  can  be  supplied  as  the  prior  for  incorporating  the  first 
batch  of  measurements.  We  set  the  covariance  of  this  prior  to  <toI,  with  do  a  large  scalar,  which  makes 
the  prior  diffuse.  The  large  covariance  allows  measurements  to  override  the  positions  prescribed  by 
the  prior,  but  provides  a  sensible  default  when  few  measurements  are  available.  The  mode  of  this 
prior  (or  for  subsequent  iterations,  the  mode  of  p(s\yold ))  is  also  used  as  the  initial  iterate  for  the 
Newton-Raphson  iterations.  To  obtain  the  initial  iterate  an  event,  we  use  the  average  estimated 
location  of  the  three  sensors  with  the  smallest  range  measurements  to  the  event. 

In  our  experiments,  we  utilize  the  radio  connectivity  of  the  sensors  to  obtain  prior  localization 
information.  The  initialization  step  described  by  Priyantha  et  al.  [14]  provides  rough  position 
estimates  to  serve  as  a  prior  before  any  measurements  are  introduced.  This  prior  takes  the  form 
^(s*)  oc  exp  —  ^  Yli  llsf  ~  xi  II 2  j  where  x °  is  the  position  predicted  by  the  initialization  step  of 
the  ith  sensor  and  ero  is  a  large  variance. 
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Because  the  sensor  nodes  are  nearly  identical,  we  know  a  priori  that  the  variation  between  their 
calibration  parameters  is  small.  These  small  variations  are  due  mainly  to  environmental  effects,  so 


sensors  that  are  close  together  tend 
in  a  prior  of  the  form  p(se)  oc  exp 
are  in  close  proximity  to  each  other. 


to  have  similar  calibration  values.  We  encode  this  information 
where  the  summation  is  over  sensors  that 


-2  Ei-i  (*?-*?) 


4  Distributing  LaSLAT 

Though  our  current  implementation  sends  measurement  batches  to  a  central  computer,  we  show 
here  that  LaSLAT  can  be  feasibly  distributed  if  desired.  LaSLAT  consists  of  two  significant  com¬ 
putational  steps:  incorporating  measurements  and  applying  the  dynamics  model.  As  formulated  in 
this  paper,  these  steps  can  be  performed  using  only  local  communication  between  sensors  that  have 
witnessed  a  common  event.  Furthermore,  the  prior  p(x*|yoW)  that  encodes  LaSLAT’s  state  between 
batches  distributes  across  the  network  in  a  straightforward  way.  We  assume  that  since  events  are 
heard  only  by  sensors  near  the  mobile,  it  is  very  likely  that  any  two  sensors  that  witness  the  same 
event  are  also  within  radio  communication  range. 

The  prior  is  completely  summarized  by  a  vector  of  means  x4*  and  an  inverse  covariance  Q.  The 
mean  vector  is  trivial  to  distribute,  since  each  sensor  can  simply  store  its  own  mean.  Q  requires 
more  careful  consideration,  but  is  also  distributable.  The  symmetric  inverse  covariance  matrix 
^  =  [s?s  rf  ]  defines  an  undirected  graph  between  sensors  and  measurements.  Two  vertices  in  this 
graph  are  connected  if  their  corresponding  entry  in  Q  is  non-zero.  We  say  fl  has  local  connectivity 
if  the  corresponding  graph  only  connects  sensors  that  are  within  measurement  range  of  each  other, 
and  connects  events  only  to  the  sensors  that  measured  the  event.  Then,  if  has  local  connectivity, 
each  row  of  fl  corresponding  to  a  sensor  has  about  niocai  +  nevents  non-zero  entries,  where  niocai 
is  the  sensor’s  number  of  one-hop  neighbors,  and  nevents  is  the  number  of  events  observed  by 
the  sensor  in  the  most  recent  batch.  Each  row  corresponding  to  an  event  has  about  niocai  non¬ 
zero  elements,  since  only  sensors  within  the  neighborhood  of  the  event  obtain  measurements  to  it. 
Locally  connected  matrices  are  therefore  easy  to  distribute.  Each  sensor  stores  its  own  rows  in  Q, 
and  the  rows  of  some  fraction  of  the  events  it  has  witnessed.  The  amount  of  data  stored  by  each 
sensor  is  0(niocai+nevents)-  However,  for  each  network,  niocai  and  nevents  are  fixed  constants.  After 
deployment,  therefore,  the  amount  of  data  stored  by  a  sensor  is  0(1). 

It  remains  to  be  shown  that  the  LaSLAT  computations  require  only  local  communication  and 
retain  local  connectivity  in  0.  The  latter  is  easily  confirmed  by  induction  on  The  initial  prior 
has  f ls  =  do/,  which  is  diagonal  and  therefore  locally  connected.  As  we  show  in  the  appendix,  the 
measurement  incorporation  and  Gaussian  approximation  steps  do  not  change  the  connectivity  of  Q. 
The  connectivity  of  Qs  only  changes  when  events  are  marginalized  out  of  the  Gaussian  prediction  by 
equation  (6).  It  can  be  verified,  however,  that  — term  added  to  only  affects  elements 
whose  corresponding  sensors  observed  an  event  in  common  during  the  most  recent  batch.  As  a 
result,  retains  local  connectivity  during  LaSLAT  operations. 

Finally,  we  will  consider  the  time  complexity  of  LaSLAT  operations  and  show  that  LaSLAT 
requires  only  local  communication  between  sensors.  The  two  significant  operations  performed  during 
each  LaSLAT  batch  are  Newton-Raphson  iterations  (12)  and  event  marginalization  (6). 

Each  Newton-Raphson  iteration  has  two  parts.  First  a  matrix  and  vector  must  be  computed 


based  on  equation  (12).  Then,  a  least  squares  optimization  must  be  performed.  The  matrix  and 
vector  can  be  computed  locally,  since  they  require  only  the  parts  of  f 1  and  x**  that  are  found 
locally  and  the  measurements  to  any  local  events.  The  communication  and  time  costs  for  each 
sensor  are  proportional  to  nevents  *  niocah  which  is  required  to  broadcast  new  measurements  to 
neighboring  sensors.  Once  the  matrix  and  vector  are  computed,  the  least  squares  optimization 
can  be  performed  using  Gauss-Seidel  iterations  [21].  Gauss-Seidel  is  guaranteed  to  converge  when 
solving  symmetric  positive  definite  systems  of  equations  like  ours.  Each  iteration  of  Gauss-Seidel 
requires  0(niocai)  computation  and  0(1)  radio  messages  per  sensor  and  event.  In  practice,  we 
find  that  Gauss-Seidel  converges  in  3-4  iterations  for  our  systems,  since  LaSLAT  does  not  require 
high  precision  convergence.  Gauss-Seidel  does  require  that  sensors  perform  their  processing  in  a 
consistent  order,  which  diminishes  the  potential  parallelization  of  the  least  squares  computation. 
However,  with  a  constant  bound  on  the  number  of  Gauss-Seidel  iterations,  the  total  time  required 
for  each  Newton- Raphson  iteration  is  0{n  *  niocai),  where  n  is  the  number  of  sensors  and  events 
whose  parameters  are  updated  by  the  iteration.  Note  that  this  is  not  a  tight  upper  bound:  as  the 
sensor  parameters  begin  to  converge,  many  parameters  will  not  need  to  be  updated  every  iteration. 
This  increases  the  amount  of  parallelism  that  can  be  exploited,  allowing  the  total  running  time  to 
approach  0(niocai),  the  amount  of  time  required  to  simply  locate  the  newest  events. 

Marginalization  is  distributable,  since  each  sensor  row  is  updated  only  on  behalf  of  local  events. 
Since  Hse  and  Qes  are  sparse,  and  is  diagonal,  the  total  time  required  is  only  0{niocai  *  nevents) 
per  sensor.  All  the  computations  can  occur  in  parallel. 

As  we  have  observed,  niocai  and  nevents  are  user-defined  constants  for  each  network.  This  leads 
us  to  believe  that  a  distributed  implementation  of  LaSLAT  is  feasible  and  would  perform  tolerably. 
We  have  not  yet  had  the  opportunity  to  test  our  distributed  algorithm  on  a  real  network  -  we  hope 
to  do  so  in  the  future.  In  our  results,  we  perform  all  our  computations  on  a  centralized  computer. 
Many  of  the  sparse  matrix  optimizations  described  in  this  section  yield  substantial  performance 
improvements  in  a  centralized  context  as  well. 

5  Results 

Our  experiments  use  the  Cricket  ranging  system  [10].  Sensor  Crickets  are  placed  on  the  floor,  and 
one  Cricket  is  attached  to  a  mobile.  The  mobile  Cricket  emits  an  event  (a  radio  and  ultra-sound 
pulse)  every  second.  The  difference  in  arrival  time  of  these  two  signals  to  a  sensor  is  proportional 
to  the  distance  between  the  sensor  and  mobile.  Crickets  compute  ranges  from  these  arrival  times. 
No  range  measurements  between  the  Crickets  on  the  floor  were  collected.  The  measurements  are 
transmitted  to  a  desktop  machine,  which  processes  them  in  batches  using  LaSLAT.  The  ultra-sound 
sensor  on  a  Cricket  occupies  a  1  cm  by  2  cm  area  on  the  circuit  board,  so  it  difficult  to  estimate 
the  ground  truth  location  of  a  Cricket  beyond  that  accuracy. 

Our  first  experiment  uses  the  same  setup  as  [15].  Six  sensor  crickets  were  placed  around  a 
rectangular  enclosure  2.1  meters  by  1.6  meters.  A  ROOMBA  robotic  vacuum  cleaner  with  the 
mobile  Cricket  attached  was  allowed  to  move  freely  within  the  enclosure,  generating  about  250 
events.  See  Figure  1(a). 

Most  events  were  measured  by  all  6  sensors.  An  initial  localization  guess  was  obtained  from  radio 
connectivity  information  using  the  initialization  routine  of  [14].  The  resulting  average  localization 
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(a)  (b) 


Figure  1:  (a)  Small  network  setup.  Six  sensors  (squares)  are  arranged  around  a  rectangular  enclo¬ 
sure.  A  camera  captured  the  ground  truth  trajectory  of  the  ROOMBA.  The  ROOMBA  followed 
the  trajectory  depicted,  (b)  Recovered  trajectory  and  sensor  positions.  Circles  are  guesses  of  initial 
sensor  locations  obtained  from  radio  connectivity.  LaSLAT  processed  measurements  in  batches  of 
30,  and  recovered  sensor  locations  depicted  by  crosses.  The  trajectory  is  also  correctly  recovered. 
LaSLAT  improves  considerably  on  the  initial  localization  guess  obtained  from  connectivity.  After 
a  global  rotation  and  translation,  the  average  localization  error  for  the  sensors  was  1.8  cm,  which 
is  within  the  error  tolerance  of  the  ground  truth. 
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(a)  (b) 

Figure  2:  (a)  Sensor  locations  and  mobile  trajectory  for  the  large  network.  Blue  circles  outline 
each  of  the  27  sensor  nodes.  Markers  on  the  trajectory  depict  the  location  of  events.  250  of  the 
1500  events  are  shown,  with  consecutive  events  connected  by  a  line.  The  mobile  was  offset  from  the 
ground  plane  and  could  pass  over  nodes.  To  generate  this  figure,  a  homography  that  accounts  for 
the  camera  transformation  was  used  to  project  real-world  coordinates  to  image  coordinates.  (b) 
Final  LaSLAT  localization  result,  with  batch  size  of  10.  Crosses  show  estimated  sensor  locations. 
These  are  correctly  estimated  to  fall  on  the  corresponding  sensor.  Average  localization  is  1.9  cm. 


error  in  this  initial  guess  was  66  cm.  This  initial  guess  was  used  as  a  prior  and  an  initial  iterate  for 
LaSLAT.  LaSLAT  incorporated  range  measurements  in  batches  of  30.  Each  mode  finding  operation 
required  an  average  of  only  2.8  Newton- Raphson  steps.  Figure  1(b)  shows  the  estimated  sensor 
localizations  and  trajectory.  Since  the  output  had  an  arbitrary  rotation  and  translation,  it  was 
rigidly  aligned  to  fit  the  rotation  and  origin  of  the  ground  truth  using  the  algorithm  described 
in  [22].  The  final  localization  error  was  1.8  cm,  averaged  over  the  sensor  nodes.  This  is  within  the 
error  tolerance  of  the  ground  truth. 

Our  second  experiment  involved  a  larger  network  with  27  Cricket  sensor  nodes  deployed  in 
a  7  m  by  7  m  room.  Whereas  in  the  previous  experiment  the  nodes  were  on  the  perimeter  of 
the  ROOMBA  mobile’s  trajectory,  in  this  experiment,  we  manually  pushed  a  mobile  through  the 
network,  generating  about  1500  events.  Figure  2(a)  shows  the  location  of  the  sensors  and  part  of 
the  trajectory  of  the  mobile  projected  on  a  top  view  picture  of  the  setup. 

Each  event  was  heard  by  about  10  sensors.  Figures  3(a)-(d)  show  localization  and  tracking 
output  as  event  batches  are  processed,  along  with  the  ground  truth  and  estimated  mobile  trajectories 
for  that  batch.  Error  ellipses  show  unit  standard  deviation  contours  for  each  sensor  node.  Nodes 
have  high  uncertainty  at  early  stages,  but  when  the  mobile  passes  near  a  node,  its  error  ellipse 
shrinks  appropriately.  In  this  experiment,  a  measurement  bias  of  about  23  cm  was  computed  for 
each  sensor  node.  Figure  2(b)  shows  the  final  localization  of  the  nodes,  reprojected  on  the  picture  of 
the  setup.  This  experiment  used  batches  of  10  measurements  and  produced  a  final  localization  error 
of  1.9  cm.  Since  the  ground  truth  is  only  accurate  to  a  few  centimeters,  localization  performance  is 


11 


Figure  3:  The  output  of  LaSLAT  after  incorporating  (a)  50,  (b)  120,  (c)  160,  and  (d)  1510  events. 
The  batch  size  was  10.  Recovered  mobile  trajectory  (crosses)  and  ground  truth  mobile  trajectory 
(solid  line)  for  the  latest  batch  are  connected  by  a  line  to  show  correspondences.  Estimated  mo¬ 
bile  locations  (dark  rectangles)  and  the  ground  truth  mobile  locations  (light  rectangles)  are  also 
connected  with  a  line  to  show  correspondence.  Error  ellipses  shrink  as  more  data  becomes  avail¬ 
able.  Between  events  120  and  160  (subfigures  (b)  and  (c)),  the  mobile  swept  around  the  bottom  of 
the  network,  and  the  error  ellipses  and  localization  error  diminished  for  those  sensors.  As  sensors 
became  better  localized,  tracking  improved. 


best  examined  visually  via  Figure  2(b). 

We  compared  the  Extended  Kalman  Filter,  which  is  LaSLAT  limited  to  one  Newton-Raphson 
iteration,  to  LaSLAT  operating  with  different  batch  sizes.  Figure  4  shows  average  localization  errors 
as  events  were  processed.  The  EKF  performs  best  with  no  batching  (batch  size  =  1).  LaSLAT 
converges  faster  and  also  exhibits  lower  steady  state  localization  error.  As  batch  sizes  are  increased, 
so  does  the  rate  of  convergence  of  LaSLAT.  Batching  also  improves  the  final  localization  error. 
LaSLAT,  with  batch  sizes  of  1,  10  and  40,  produced  final  localization  errors  of  3  cm,  1.9  cm,  and 
1.6  cm  respectively.  On  average,  LaSLAT  took  3  Newton-Raphson  iterations  to  incorporate  each 
batch.  The  EKF’s  final  localization  error  was  7.5  cm,  which  is  outside  the  error  tolerance  for  the 
ground  truth. 

We  simulated  the  distributed  version  of  our  algorithm  described  in  Section  4  using  various  batch 
sizes.  In  this  simulation,  we  replaced  the  linear-system  solver  used  in  each  Newton-Raphson  iteration 
with  a  solver  that  uses  Gauss-Seidel  iteration  and  hence  could  be  distributed  in  a  straightforward 
way.  Our  simulations  show  that  on  average,  only  three  to  four  Gauss-Seidel  iterations  are  sufficient 
to  implement  each  Newton-Raphson  iteration,  for  a  total  average  of  about  9  Gauss-Seidel  iterations 
for  processing  each  batch.  Gauss-Seidel  based  localization  and  tracking  convergence  and  accuracy 
was  identical  to  the  centralized  computations  reported  in  this  section. 

Figure  5  shows  localization  results  on  a  larger  network  (49  sensors)  deployed  over  a  larger  area 
(10  m  by  17  m).  With  about  0.3  sensors  per  square  meter,  this  network  is  about  half  as  dense  as 
the  one  shown  in  Figure  2,  which  had  about  0.5  sensors  per  square  meter.  As  a  result,  on  average 
only  5  sensors  heard  each  event,  and  the  localization  error  was  about  7.5  cm.  The  algorithm  also 
determined  a  measurement  biases  of  about  20  cm  for  all  nodes.  For  all  batch  sizes,  the  EKF  produced 
an  average  localization  error  of  about  80  cm,  showing  that  the  improvement  due  to  Laplace’s  method 
can  be  very  significant. 
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Figure  4:  Localization  error  as  a  function  of  the  number  of  events  observed  for  EKF  and  various  batch 
sizes  for  LaSLAT.  LaSLAT  converges  more  quickly  and  attains  a  lower  steady  state  error  than  the  EKF. 
Furthermore,  larger  batch  sizes  improve  the  convergence  rate  and  the  steady  state  error  of  LaSLAT. 


Figure  5:  LaSLAT  localization  result  on  a  sparser  sensor  network  with  49  nodes  in  a  10m  by  17m 
environment.  Crosses  indicate  the  recovered  sensor  locations,  projected  onto  the  image.  The  average 
localization  error  was  7.5  cm. 
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Figure  6:  Environment  used  for  initial  3d  localization  experiments.  LaSLAT  localized  sensors  to 
within  7  cm. 

5.1  Extending  LaSLAT  to  3D 

It  is  possible  to  use  LaSLAT  to  localize  and  track  sensors  and  mobiles  in  a  three  dimensional 
environment.  In  our  initial  experiments,  we  placed  40  crickets  on  the  floor  and  walls  of  a  4  x  6 
meter  room,  which  contained  all  of  its  normal  furniture:  tables,  chairs,  printers,  and  a  refrigerator. 
The  mobile  was  a  special  cricket  with  additional  ultrasound  transmitters  attached  so  that  it  could 
broadcast  in  all  directions.  This  mobile  was  carried  by  hand  through  the  room  and  moved  completely 
arbitrarily,  including  changes  in  speed,  loops,  and  twists. 

We  made  several  adjustments  to  better  accomodate  the  new  environment.  First,  the  measure¬ 
ment  model  was  extended  to  detect  and  reject  range  measurement  outliers  due  to  ultrasound  echoes. 
In  the  new  model,  measurements  typically  have  additive  Gaussian  noise  as  in  2d,  but  are  occasion¬ 
ally  completely  incorrect,  which  we  represent  by  a  uniform  distribution.  This  change  in  model 
required  us  to  use  an  Expectation  Maximization  (EM)  algorithm  to  optimize  equation  (1). 

Our  initial  results  suggest  that  LaSLAT  is  capable  of  achieving  very  good  results  in  three  dimen¬ 
sions.  In  the  room  shown  in  figure  6,  LaSLAT  localized  sensors  to  within  7  cm  while  successfully 
tracking  the  path  of  the  mobile  in  3d.  Much  of  this  error  is  accounted  for  by  the  difficulty  of 
measuring  ground  truth  in  this  environment. 

6  Conclusion  and  Future  Work 

We  have  demonstrated  that  adapting  Bayesian  techniques  to  SLAT  results  in  very  accurate  local¬ 
ization.  Our  algorithm  is  on-line  and  uses  a  Bayesian  filter  to  estimate  sensor  locations  and  mobile 
trajectories.  We  showed  experimentally  that  we  can  track  and  localize  sensors  to  within  one  or  two 
centimeters. 
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The  Bayesian  framework  provides  many  other  advantages  that  we  hope  to  demonstrate  in  the 
future.  The  three  dimensional  extension  described  in  our  results  section  shows  great  promise,  and 
we  look  forward  to  experimenting  with  it  further.  Also,  different  types  of  measurements  such  as 
bearings  could  be  included  by  suitably  modifying  the  measurement  model.  By  modeling  dynamics 
on  the  position  of  sensors,  sensor  may  be  allowed  to  be  move  over  time.  Using  our  framework, 
we  also  hope  to  simultaneously  calibrate  sensor  parameters  (such  as  a  time  offset  parameter  when 
computing  ranges  from  time  differences  of  arrivals  on  the  Crickets),  while  localizing  the  sensors  and 
tracking  the  mobile.  Finally,  we  hope  to  complete  a  true  distributed  implementation  of  LaSLAT  on 
a  real  sensor  network. 
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8  Appendix:  Finding  a  Mode 

The  update  (5)  can  be  rewritten  in  the  form  of  non-linear  least  squares.  Letting  ftj(x)  =  ||s?  — 
e*- 1|  +  s?,  defining  f(x)  as  a  column  vector  consisting  of  all  /)_, (x) ,  Px  =  Cov-1  [x*|yoW],  and 
/Ux  =  E  [x*|yoW],  recast  (5)  as: 

argmin  —W 1 1 f (x)  -y#||2  +  (x  -  /ux)TPx(x  -  fj,x)  (10) 

X  (JZ 

Each  iteration  of  Newton-Raphson  maps  an  iterate  x^  to  the  next  iterate  x(t+1)  by  approximating 
(10)  via  linearization  about  and  optimizing  over  x: 

x(f+1)  =  argmin  \  Vf(<)x  -  b  +  (x  -  ^x)TOx(x  -  /xx),  (11) 

where  Vf^  is  the  derivative  of  f  with  respect  to  x  at  x^,  and  b  =  Vf^x^  —  f  (x^)  —  yt. 

This  is  a  linear  least  squares  problem  in  terms  of  x.  Its  solution  can  be  found  by  setting  the 
derivative  with  respect  to  x  to  zero  to  obtain  a  linear  problem  that  can  be  solved  by  matrix  inversion: 

[P  +  Vf(t)T  Vf (t)]  x  =  Sin  +  ^ Vf(t)Tb.  (12) 

Furthermore,  differentiating  (11)  one  more  time  results  in  H  =  P  +  AVf^TVfW.  Since  (11) 
is  an  approximation  to  the  negative  log  posterior  (5),  H  serves  as  an  approximation  to  its  Hessian 
at  x^. 

Because  the  true  distance  fij  depends  only  on  sensor  i  and  event  location  j,  each  row  of  Vf^ 
is  made  up  of  zeros,  except  at  locations  corresponding  to  the  ith  sensor  and  the  jth  event.  Thus 
Vfd)TvfW  has  local  connectivity.  If  Px  =  Cov-1  [  x*|yoW]  has  local  connectivity,  then  the  updated 
covariance  matrix  Cov-1  [x*|yoM]  =  P  +  Vf^TVf^/<r2  also  has  local  connectivity.  Therefore 
incorporating  a  batch  of  measurements  preserves  local  connectivity. 
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